#include "MK20D5.h"           // Device header
#include "uart.h"							// Serial driver

#define BENCH_RUNS 100        /* Not really needed unless OS used,
															   but makes for a nice blink rate ;) */

// Benchmark externals
extern void benchmark(void);
extern int16_t* benchmark_data(void);
extern unsigned benchmark_size(void);

volatile unsigned count;			// Performance counter
volatile unsigned cntmsb;     // FTM0 MSB counter

// FTM0 overflow (MSB) counter
// Note: FTM0 is only 16 bits, this is used to extend to 32 bits.
//       The timer must be is stopped before reading.
void FTM0_IRQHandler(void) {
	FTM0->SC &= ~FTM_SC_TOF_MASK; // Clear overflow
	cntmsb++; // MSB counter
}

// System setup
void setup(void) {
  // Clock gates 
	SIM->SCGC5 |= SIM_SCGC5_PORTA_MASK | SIM_SCGC5_PORTB_MASK | SIM_SCGC5_PORTC_MASK | SIM_SCGC5_PORTD_MASK;
	SIM->SCGC6 |= SIM_SCGC6_FTM0_MASK;
  // Configure PORT module
	PORTD->PCR[ 4] = PORT_PCR_MUX(1); // PTD4
  PORTC->PCR[ 3] = PORT_PCR_MUX(1); // PTC3
  PORTA->PCR[ 2] = PORT_PCR_MUX(1); // PTA2
	PORTB->PCR[16] = PORT_PCR_MUX(3); // UART0_RX
	PORTB->PCR[17] = PORT_PCR_MUX(3); // UART0_TX
	// Configure GPIO module
  PTD->PDOR = (1UL << 4); PTC->PDOR = (1UL << 3); PTA->PDOR = (1UL << 2);
  PTD->PDDR = (1UL << 4); PTC->PDDR = (1UL << 3); PTA->PDDR = (1UL << 2);
	// Configure FTM module (core clock)
	FTM0->MOD = 0xFFFF;
  NVIC_EnableIRQ(FTM0_IRQn);
  // Set up uart
	uart_init(42940000, 115200);
}

// Reset benchmark timer
__inline
void time_reset(void) {
	FTM0->SC = 0; // Disable
	cntmsb = 0; // Clear MSB
	FTM0->CNT = 0; // Clear LSB
  FTM0->SC = FTM_SC_CLKS(1) | FTM_SC_TOIE_MASK; // Enable
}

// Read benchmark timer
__inline
unsigned time_read(void) {
  FTM0->SC = 0; // Disable
	return FTM0->CNT | (cntmsb << 16); // Combine MSB|LSB
}

// LED indication - red, green, blue
void led_r(void) { PTA->PSOR = (1UL << 2); PTD->PSOR = (1UL << 4); PTC->PCOR = (1UL << 3); }
void led_g(void) { PTC->PSOR = (1UL << 3); PTA->PSOR = (1UL << 2); PTD->PCOR = (1UL << 4); }
void led_b(void) { PTD->PSOR = (1UL << 4); PTC->PSOR = (1UL << 3); PTA->PCOR = (1UL << 2); }

// Hex characters
const char hex[] = "0123456789ABCDEF";

// Print string
void print(char *s) {
	while(*s != 0) uart_putch(*s++);
}

// Print number
void print_num(unsigned n) {
	char temp[10], *ptr = &temp[sizeof(temp)];
	do { *--ptr = '0' + (n % 10); n /= 10; } while(n);
	while(ptr < &temp[sizeof(temp)]) uart_putch(*ptr++);
}

// Report to serial
void report() {
	print("FFT output:\n  ");
	int16_t *ptr = benchmark_data();
	unsigned len = benchmark_size();
	while(len) {
		uart_putch(hex[(*ptr   >> 12) & 0xF]);
		uart_putch(hex[(*ptr   >>  8) & 0xF]);
		uart_putch(hex[(*ptr   >>  4) & 0xF]);
		uart_putch(hex[(*ptr++ >>  0) & 0xF]);
		uart_putch(' ');
		if(0 == (--len & 7) && len != 0) print("\n  ");
	}
	print("\n\nResults:\n  CPU cycles  : ");
	print_num(count);
	print("\n  Time @ 48MHz: ");
	print_num(count / 48000);
	uart_putch('.');
	print_num((count / 4800) % 10 + (((count / 480) % 10) > 4 ? 1 : 0));
	print("mS\n\n\n");
}

// Entry
int main (void) {
  setup(); // Board setup

	unsigned toggle = 0;
	while(1) {

		// Toggle RED/GREEN to indicate activity
		toggle = !toggle;
		if(toggle) led_r(); else led_g();

		// Run benchmark
		unsigned ack = 0;
		for(unsigned n = 0; n < BENCH_RUNS; n++) {
			time_reset(); // Reset timer
			benchmark(); // Run test
			ack += time_read(); // Accumulate time
		}
		// Divide to get time of a single run
		count = ack / BENCH_RUNS;
		// Set BLUE to indicate done
		led_b();
		// Report to serial
		report();

		// BREAKPOINT HERE - check "count"
		count = count;
		// For -O3, optimize for time, cross module optimization:
		//   Original count: 0x279AD
		//   Translates to : 3.4mS @ 48MHz (0x279AD/48MHz) or 295 runs/second

	}
}
